//头文件
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include<iostream>
using namespace cv;
class use_kalman_filter
{
private:
    Mat Q;			//系统噪声方差矩阵Q
	Mat R;			//测量噪声方差矩阵R
	Mat F;			//状态转移矩阵，物理方程
	Mat H;          //观测矩阵
	Mat K;			//卡尔曼系数

	Mat P;			//预测的协方差矩阵
	Mat P_predict;  //最终的协方差矩阵
	Mat x_hat_prect;
	Mat temp;
public:
	Mat x_state;
	Mat z;						//真实值
	int stateNum;					//状态值个数
	int measureNum;					//测量值个数

    void set_Q(float x)
    {
        Q = x*Mat::eye(stateNum, stateNum, CV_32F);
    }
    
    void set_R(float y)
    {
    
        R = y*Mat::eye(measureNum, measureNum, CV_32F);
    
    }
    
    
    void set_P(float z)
    {
    
        P_predict = z*Mat::eye(stateNum, stateNum, CV_32F);
    
    }
    
    void get_F(Mat FF)
    {
        F = FF.clone();
    }
    
    
    void init()
    {
        K = Mat::zeros(stateNum, stateNum, CV_32F);
        H = Mat::zeros(measureNum, stateNum, CV_32F);
        temp = Mat::zeros(stateNum, stateNum, CV_32F);
    
        for (int i = 0; i < measureNum; i++)
        {
            H.at<float>(i, i) = 1;
        }
    
    }
    
    void correct()
    {
        //公式均为blog中的公式
    
        //predict
        x_hat_prect = F*x_state;
        P = F*P_predict*F.t() + Q;
    
        //Update
        temp = H*P*H.t() + R;
        temp = temp.inv();
        K = P*H.t() *temp;
    
        x_state = x_hat_prect+ K*(z - H *x_hat_prect);     				//预测值
        P_predict = (Mat::eye(stateNum, stateNum, CV_32F) - K*H)*P;		//预测值协方差
    
    }
    //用于对预测值的更新
};




